//#include <math.h>
//#include "main.h"
//#include "DriversStruct.h"
//#include "Quat-Kalman.h"
//#include "Kalman.h"

//Quaternion q;       // 四元数结构体
//EulerAngles angles; // 姿态角结构体

//// 初始化四元数
//void quaternion_init(Quaternion *q)
//{
//    q->w = 1.0f;
//    q->x = 0.0f;
//    q->y = 0.0f;
//    q->z = 0.0f;
//}

//// 四元数乘法
//Quaternion quaternion_multiply(Quaternion q1, Quaternion q2)
//{
//    Quaternion result;
//    result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
//    result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
//    result.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
//    result.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
//    return result;
//}

//// 四元数归一化
//void quaternion_normalize(Quaternion *q)
//{
//    float norm = sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
//    q->w /= norm;
//    q->x /= norm;
//    q->y /= norm;
//    q->z /= norm;
//}

//// 四元数转欧拉角
//EulerAngles quaternion_to_euler(Quaternion q)
//{
//    EulerAngles angles;
//    angles.roll = atan2(2 * (q.w * q.x + q.y * q.z), 1 - 2 * (q.x * q.x + q.y * q.y)) * RAD_TO_DEG;
//    angles.pitch = asin(2 * (q.w * q.y - q.z * q.x)) * RAD_TO_DEG;
//    angles.yaw = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)) * RAD_TO_DEG;
//    return angles;
//}

//// 卡尔曼滤波更新
//void kalman_filter_update(Quaternion *q, BMI088_ReceiveDataTypedef *BMI088, uint32_t *timer)
//{

//    // 计算时间增量 dt，单位为秒
//    double dt = (double)(HAL_GetTick() - *timer) / 1000;
//    *timer = HAL_GetTick();

//    // 将陀螺仪的单位从度每秒转换为弧度每秒
//    BMI088->gyro_x *= DEG_TO_RAD;
//    BMI088->gyro_y *= DEG_TO_RAD;
//    BMI088->gyro_z *= DEG_TO_RAD;

//    // 预测步骤
//    Quaternion dq;
//    dq.w = 0;
//    dq.x = 0.5f * BMI088->gyro_x * dt;
//    dq.y = 0.5f * BMI088->gyro_y * dt;
//    dq.z = 0.5f * BMI088->gyro_z * dt;
//    *q = quaternion_multiply(*q, dq);
//    quaternion_normalize(q);

//    // 测量步骤
//    float norm = sqrt(BMI088->acc_x * BMI088->acc_x + BMI088->acc_y * BMI088->acc_y + BMI088->acc_z * BMI088->acc_z);
//    BMI088->acc_x /= norm;
//    BMI088->acc_y /= norm;
//    BMI088->acc_z /= norm;

//    float vx = 2 * (q->x * q->z - q->w * q->y);
//    float vy = 2 * (q->w * q->x + q->y * q->z);
//    float vz = q->w * q->w - q->x * q->x - q->y * q->y + q->z * q->z;

//    float ex = (BMI088->acc_y * vz - BMI088->acc_z * vy);
//    float ey = (BMI088->acc_z * vx - BMI088->acc_x * vz);
//    float ez = (BMI088->acc_x * vy - BMI088->acc_y * vx);

//    static float e_int_x = 0;
//    static float e_int_y = 0;
//    static float e_int_z = 0;

//    e_int_x += ex * dt;
//    e_int_y += ey * dt;
//    e_int_z += ez * dt;

//    BMI088->gyro_x += Q * ex + e_int_x;
//    BMI088->gyro_y += Q * ey + e_int_y;
//    BMI088->gyro_z += Q * ez + e_int_z;

//    // 更新四元数
//    Quaternion dq_updated;
//    dq_updated.w = 0;
//    dq_updated.x = 0.5f * BMI088->gyro_x * dt;
//    dq_updated.y = 0.5f * BMI088->gyro_y * dt;
//    dq_updated.z = 0.5f * BMI088->gyro_z * dt;
//    *q = quaternion_multiply(*q, dq_updated);
//    quaternion_normalize(q);
//}
